/**
 * @file   : main.cpp
 * @author : rebeater
 * @brief  : decode bin file from stm32f429 board, convert rslt(binary) to navigation file.
 * @date   : 2020/11/06
 * @version 1.1 add imd format as output
 * */
#include <iostream>
#include <fstream>
#include <cstdio>
#include <gflags/gflags.h>
#include "nav_struct.h"
#include "deque"

DEFINE_string(file, "/media/rebeater/hd_data2/workspace/raw_data/Indoor/210112/demo_2.imu", "input file path");


using namespace std;
const int nav_size = sizeof(OutputData);
char buffer[nav_size];

void convert(ImuData &imu){
    double temp;
    temp = imu.acce[0];
    imu.acce[0]=imu.acce[1];
    imu.acce[1] = temp;
    imu.acce[2] = -imu.acce[2];
    temp = imu.gyro[0];
    imu.gyro[0] = imu.gyro[1];
    imu.gyro[1] = temp;
    imu.gyro[2] = -imu.gyro[2];
}


void run(string &file) {
    ifstream is(file, std::ios::binary | std::ios::in);
    ofstream os(file + ".nav", std::ios::out);
    ofstream os_imu(file + ".imutxt", std::ios::out);
    ofstream os_imd(file + ".imd", std::ios::out | std::ios::binary);
    ofstream os_gnss(file + ".gpstxt", std::ios::out);
    ofstream os_press(file + ".prstxt", std::ios::out);

    uint8_t data;
    char str_buff[1024];
    if (!is.is_open()) {
        return;
    }
    int count = 0;
    GnssData gnss_data_pre = {0};
    while (is.good()) {
        is.read(buffer, 1);
        data = *((long *) buffer);
        if (data != 0x55) continue;
        is.read(buffer, 1);
        data = *((long *) buffer);
        if (data != 0xAA) continue;
        is.read(buffer, 1);
        data = *((long *) buffer);
        if (data != 0x55) continue;
        is.read(buffer, 1);
        data = *((long *) buffer);
        if (data != 0xAA) continue;
        is.read(buffer, 4); /*4个字节无效*/
        count++;
        is.read(buffer, sizeof(OutputData));
        OutputData outputData = *((OutputData *) buffer);
#ifdef RT_CAR
        auto nav = outputData.navEpoch;
        sprintf(str_buff, "0 %10.3f %18.12f %18.12f %10.5f %10.4f %10.4f %10.4f %10.5f %10.5f %10.5f %10.5f %10.5f %10.5f %10.5f %10.5f %10.5f %2d\n",
                nav.gpst, nav.lat / _deg, nav.lon / _deg, nav.h,
                nav.vn, nav.ve, nav.vd,
                nav.roll / _deg, nav.pitch / _deg, nav.yaw / _deg,
                nav.gb[0],nav.gb[1],nav.gb[2],
                nav.ab[0],nav.ab[1],nav.ab[2],
                (uint32_t) nav.mode);
        os << str_buff;
#endif
        auto imu = (ImuData) outputData.imuData;
        /*R F U -> F R D*/
        convert(imu);
        sprintf(str_buff, "%12.4f %10.7f %10.7f %10.7f %10.7f %10.7f %10.7f\n",
                imu.gpst, imu.gyro[0], imu.gyro[1], imu.gyro[2], imu.acce[0], imu.acce[1],
                imu.acce[2]);
        if(imu.gpst>1000){
            os_imu << str_buff;
            os_imd.write((char *) &imu, sizeof(ImuData));
        }

        auto gnss_data = outputData.gnssData;
        if (gnss_data.gpst > gnss_data_pre.gpst + 0.5) {
            sprintf(str_buff, "%12.4f %13.9f %13.9f %10.5f %6.3f %6.3f  %2d %2d\n", gnss_data.gpst,
                    gnss_data.lat / _deg,
                    gnss_data.lon / _deg, gnss_data.h, gnss_data.v_dop, gnss_data.h_dop, gnss_data.mode, gnss_data.ns);
            os_gnss << str_buff;
            gnss_data_pre = gnss_data;
        }
//        if (gnss_data.gpst > 0){
//            gnss_data_pre = gnss_data;
//        }
#ifdef RT_CAR
        auto press = outputData.auxiliaryData;
        sprintf(str_buff,"%12.4f %6.2f %12.4f %12.5f\n",press.gpst,press.temperature_c,press.pressure_pa,press.velocity);
        os_press<<str_buff;
#endif
    }
    cout << "convert finished:" << count << endl;
    is.close();
    os.close();
    os_imu.close();
    os_imd.close();
    os_gnss.close();
    os_press.close();
}

int main(int argc, char *argv[]) {
    google::ParseCommandLineFlags(&argc, &argv, true);
    if (FLAGS_file.empty()) {
        cout << "输入文件路径:" << endl;
        return -1;
    }
    run(FLAGS_file);
    uint8_t a = 1;
    cout << !a << endl;

    return 0;
}
